#coding: utf-8
from __future__ import print_function


from pylab import *
import sys
from printyork import matrix_to_quaternion
from camori import EdgelSet, quaternion_to_matrix
import simplejson
import scipy.io
import scipy.optimize


from quaternion import Quat



def solve_transformation(Lq, Lr):
    Nc = Lq.shape[0]
    M = zeros((4*Nc,4))
    b = zeros(4*Nc)

    for nn in range(Nc):
        n = nn
        # M[nn*4+0] = array([ Lq[n,0],-Lq[n,1],-Lq[n,2],-Lq[n,3]])
        # M[nn*4+1] = array([ Lq[n,1], Lq[n,0], Lq[n,3],-Lq[n,2]])
        # M[nn*4+2] = array([ Lq[n,2],-Lq[n,3], Lq[n,0], Lq[n,1]])
        # M[nn*4+3] = array([ Lq[n,3], Lq[n,2],-Lq[n,1], Lq[n,0]])
        M[nn*4+0] = array([ Lq[n,0],-Lq[n,1],-Lq[n,2],-Lq[n,3]])
        M[nn*4+1] = array([ Lq[n,1], Lq[n,0],-Lq[n,3], Lq[n,2]])
        M[nn*4+2] = array([ Lq[n,2], Lq[n,3], Lq[n,0],-Lq[n,1]])
        M[nn*4+3] = array([ Lq[n,3],-Lq[n,2], Lq[n,1], Lq[n,0]])
        b[nn*4:nn*4+4] = Lr[n]

    #print 'Parâmetros para o mapeamento:'
    lres = lstsq(M,b)
    #print Quat(lres[0]).normalize()
    tt = Quat(lres[0]).normalize()
    return tt






def qp(a,b):
    pro =  r_[
        a[0]*b[0] - a[1]*b[1] - a[2]*b[2] - a[3]*b[3],
        a[0]*b[1] + a[1]*b[0] + a[2]*b[3] - a[3]*b[2],
        a[0]*b[2] - a[1]*b[3] + a[2]*b[0] + a[3]*b[1],
        a[0]*b[3] + a[1]*b[2] - a[2]*b[1] + a[3]*b[0]  ]
    return pro / sqrt((pro**2).sum())

def qi(a):
    return a*array([1,-1,-1,-1])

def qf(a):
    return r_[ sqrt(1-(a**2).sum()), a ]

if __name__ == '__main__':
    ion()
    finput = open(sys.argv[1])
    finput.close()


    goog = loadtxt('/home/nlw/ciencia/DADOS/street01/pose2.txt')


    set_printoptions(precision=3)



    Nf = 251
    C = zeros((Nf,4))
    D = zeros((Nf,4))

    ## Select google frames
    C[:] = goog[3500:3500+Nf, -4:] 

    # C = goog[:, -4:] 

    for n in range(Nf):
        #C[n] = (Quat(0,0,-1) * Quat(C[n])).q
        #C[n] = Quat(C[n]).q
        # C[n] = Quat(C[n]).canonical().q
        # C[n] = C[n,[0,1,2,3]]
        C[n] = C[n,[1,0,2,3]] * array([-1,-1,1,1])



    mydata = loadtxt(sys.argv[1])

    D = mydata[argsort(mydata[:,0])[:Nf], 5:9]

    D[D[:,0]<0] = -D[D[:,0]<0]


    M = dot(dot(inv(dot(D.T,D)), D.T), C)
    estC = dot(D,M)
    
    N = dot(dot(inv(dot(C.T,C)), C.T), D)
    estD = dot(C,N)


    tt = solve_transformation(C,D)
    estD = zeros((C.shape[0],4))
    for n in range(C.shape[0]):
        estD[n] = (tt * Quat(C[n])).q


    print(M)
    print(N)


    #constref = D[0]
    constref = Quat(mean(D[:,1:],0)).q

    err1 = zeros((Nf,4))
    err2 = zeros((Nf,4))
    errang1 = zeros(Nf)
    errang2 = zeros(Nf)
    for frm in range(Nf):
        errang1[frm] = (Quat(D[frm]).inverse() * Quat(estD[frm])).angle()
        errang2[frm] = (Quat(D[frm]).inverse() * Quat(constref)).angle()
        # err1[frm] = qp(D[frm], qi(estD[frm]))
        # err2[frm] = qp(D[frm], qi(constref))
    # errang1 = arcsin(sqrt((err1[:,1:]**2).sum(1)))*2*180/pi
    # errang2 = arcsin(sqrt((err2[:,1:]**2).sum(1)))*2*180/pi
    
    # figure(1, figsize=(6,4))
    figure(1, figsize=(10,7))
    suptitle(u'Parâmetros estimados e referência ajustada', fontweight='bold', size=16)

    for lab in range(4):
        subplot(2,2,lab+1)
        l1=plot(D[:,lab], 'r-', label='Estimativa')[0]
        l2=plot(estD[:,lab], 'b--', label=u'Referência')[0]
        if lab>0:
            ylim(-0.033,0.033)
        else:
            ylim(0.997,1.0)

    figlegend((l1,l2), (u'Estimativa', u'Referência'), 'lower center', ncol=2 )
    savefig('street_fit.pdf', dpi=200)

    figure(2)
    suptitle(u'Distribuição acumulada de erros', fontweight='bold', size=16)
    plot(sort(errang1), mgrid[:Nf]/float(Nf), 'r-')
    plot(sort(errang2), mgrid[:Nf]/float(Nf), 'b--')
    grid()

    legend((l1,l2), (u'Estimativa do Corisco', u'Orientação constante'), 'lower right', ncol=1 )

    savefig('street_cdf.pdf', dpi=200)





    # figure(3, figsize=(10,6))
    # suptitle(u'Parametros estimados e referência ajustada', fontweight='bold', size=20)
    # for lab in range(4):
    #     subplot(2,2,lab+1)
    #     l1=plot(estC[:,lab], 'r-', label='Estimate')[0]
    #     l2=plot(C[:,lab], 'b--', label='Reference')[0]
        # if lab>0:
        #     ylim(-0.033,0.033)
        # else:
        #     ylim(0.997,1.0)
